package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;

public class JoyDrive {
    private boolean m_reverse = false;
    private boolean m_bGigaware = false;
    private Joystick m_leftStick;
    private Joystick m_rightStick;
    private Joystick m_gigaware;
    private RobotDrive m_drivetrain;

    public JoyDrive(RobotDrive drivetrain, Joystick leftStick, Joystick rightStick, Joystick gigaware, boolean bGigaware) {
        m_drivetrain = drivetrain;
        m_leftStick = leftStick;
        m_rightStick = rightStick;
        m_gigaware = gigaware;
        m_bGigaware = bGigaware;
    }

    public void update() {
        if (m_leftStick.getRawButton(10) || m_rightStick.getRawButton(10))
            m_bGigaware = false;
        else if (m_gigaware.getRawButton(10))
            m_bGigaware = true;
        if (m_rightStick.getRawButton(5) || m_gigaware.getRawButton(6))
            m_reverse = false;
        else if (m_leftStick.getRawButton(5) || m_gigaware.getRawButton(5))
            m_reverse = true;
        if (!m_reverse) {
            if (!m_bGigaware) {
                m_drivetrain.tankDrive(m_leftStick.getY(), m_rightStick.getY());
            }
            else {
                m_drivetrain.tankDrive(m_gigaware.getY(), m_gigaware.getRawAxis(5));
            }
        }
        else {
            if (!m_bGigaware) {
                m_drivetrain.tankDrive(-m_rightStick.getY(), -m_leftStick.getY());
            }
            else {
                m_drivetrain.tankDrive(-m_gigaware.getRawAxis(5), -m_gigaware.getY());
            }
        }
    }

    public void setDrivetrain(RobotDrive drivetrain) {
        m_drivetrain = drivetrain;
    }

    public RobotDrive getDrivetrain() {
        return m_drivetrain;
    }
}